//-----------------------------------------------------------------
// Includes
//-----------------------------------------------------------------
#include "Processor.hpp"
#include "../Vision/VisionTools.hpp"
#include "../Tools/Logger.hpp"
#include "../Tools/mat44.hpp"
#include <sstream>

//#define DEBUG_GRAPHIC 1 

namespace Calibration
{	
	///////////////////////////////////////////////////////////////////
	// Constructeur
	///////////////////////////////////////////////////////////////////
	CalibrationProcessor::CalibrationProcessor()
	{
	}
	
	///////////////////////////////////////////////////////////////////
	// Destructeur
	///////////////////////////////////////////////////////////////////
	CalibrationProcessor::~CalibrationProcessor()
	{
	}
	
	///////////////////////////////////////////////////////////////////
	// Recherche les points de la mire dans l'ensemble des images du 
	// jeu d'image.
	// 
	// Retourne une matrice 2xN contenant les coordonnées 2D des points 
	// trouvées de toutes les images.
	// Retourne une matrice 1xN contenant le nombre de points trouvés 
	// pour chaque image.
	//
	// Pre condition : la mire est totalement visible sur toutes les 
	// 				   images du jeu de données
	///////////////////////////////////////////////////////////////////
	bool CalibrationProcessor::FindPatternPoints(	const Pattern & pattern, CalibrationDataset & data,
							CvMat** coordinatesFoundPoints, CvMat** nbFoundPoint )
	{
		// Déclaration
		CvPoint2D32f* cornersCordinates; 	// Contient tous les points 2D de la mire qui ont été trouvés pour chaque mire
		int nbCornersFound;			// Contient le nombre de points trouvés dans une image
		IplImage* temporaryImage;		// Image temporaire pour l'affichage des points trouvés
		int boundIndexCornersFound;		// Indice des coordonnées des points trouvés pour la matrice de retour
		

		// Initilialisation
		cornersCordinates = new CvPoint2D32f[pattern.GetNumberOfInnerCorners()];
		nbCornersFound = 0;
		boundIndexCornersFound = 0;
		(*coordinatesFoundPoints) = cvCreateMat((data.GetNumberOfImages()*pattern.GetNumberOfInnerCorners()),2,CV_32FC1);
		(*nbFoundPoint) = cvCreateMat(data.GetNumberOfImages(),1,CV_32SC1);
#ifdef DEBUG_GRAPHIC
		Tools::ILogger::GetLogger("Console")->Info( "Corner Per Rows : %d\n", pattern.GetNumberOfInnerCornersPerRow() );
		Tools::ILogger::GetLogger("Console")->Info( "Corner Per Columns : %d\n", pattern.GetNumberOfInnerCornersPerColumn() );
		Tools::ILogger::GetLogger("Console")->Info( "Corner : %d \n", pattern.GetNumberOfInnerCorners() );	
		Tools::ILogger::GetLogger("Console")->Info( "Size : %f\n", pattern.GetSquareSize() );
		Tools::ILogger::GetLogger("Console")->Info( "Nb Images : %d\n", data.GetNumberOfImages() );
#endif

		// Recherche des points de la mire pour chacune des images
		for( int index = 0; index < data.GetNumberOfImages(); index++ )
		{	
			// Recherche des points de la mire
			int allFound =	cvFindChessboardCorners(	data[index],
									cvSize(	pattern.GetNumberOfInnerCornersPerColumn(),
										pattern.GetNumberOfInnerCornersPerRow() ),
									cornersCordinates,&nbCornersFound,0 );
			
			// Recherche plus approfondie des points de la mire (recherche en 100 itération et avec une précision à 0.1)
			cvFindCornerSubPix(	data[index], cornersCordinates,
						nbCornersFound, cvSize(5,5), cvSize(-1,-1),
						cvTermCriteria(CV_TERMCRIT_ITER,100,0.1) );
#ifdef DEBUG_GRAPHIC
			// Construction et affichage de l'image avec les points détectés
			temporaryImage = cvCloneImage(	data[index]);			
							cvDrawChessboardCorners(temporaryImage,
								cvSize(	pattern.GetNumberOfInnerCornersPerRow(),
									pattern.GetNumberOfInnerCornersPerColumn()),
									cornersCordinates,nbCornersFound,allFound );
			Vision::VisionTools::DrawImage( "Calibration::FindPattern", 1000, temporaryImage );
			cvReleaseImage(&temporaryImage);
#endif
			if( allFound == 0 )
			{	
				Tools::ILogger::GetLogger("Console")->Info( "Nb Found Point: %d\n", nbCornersFound );
				return false; 
			} 

			// Construction du vecteur contenant le nombre de points trouvés				
			int * dataNbCornersFound = (*nbFoundPoint)->data.i;
			dataNbCornersFound[index] = nbCornersFound;
			
			// Construction de la matrice contenant les coordonnées des points trouvés 
			for( int indexCornersFound = boundIndexCornersFound; 
				 indexCornersFound < (boundIndexCornersFound+pattern.GetNumberOfInnerCorners()); 
				 indexCornersFound++ )
			{
				cvmSet((*coordinatesFoundPoints),indexCornersFound,0,cornersCordinates[(indexCornersFound-boundIndexCornersFound)].x);
				cvmSet((*coordinatesFoundPoints),indexCornersFound,1,cornersCordinates[(indexCornersFound-boundIndexCornersFound)].y);
			}
			boundIndexCornersFound += pattern.GetNumberOfInnerCorners();
		}
		
		// Libération des ressources allouées
		delete[] cornersCordinates;
		return true;
	}
	
	///////////////////////////////////////////////////////////////////
	// Processus de calibrage
	///////////////////////////////////////////////////////////////////
	void CalibrationProcessor::CalibrateCamera( const Pattern & pattern, CalibrationPositionSet & positionSet, CalibrationDataset & data, Vision::Camera & camera ) throw (Tools::CalibrationFailed)	
	{				
		// Recherche de l'ensemble des points dans le jeu d'images
		CvMat * imagePoints;
		CvMat * pointCounts;
		if( !FindPatternPoints( pattern, data, &imagePoints, &pointCounts) )
		{			
			throw Tools::CalibrationFailed("Some pattern's points aren't found in images");
		}					
		
		// Construction de la matrice globale des coordonnées 3D des points de la mire
		CvMat * objectPoints;	
		int nbTotalInnerCorners;		
		nbTotalInnerCorners = pattern.GetNumberOfInnerCorners()*data.GetNumberOfImages();
		objectPoints = cvCreateMat(nbTotalInnerCorners,3,CV_32FC1);				
		for( int index3DPoint = 0; index3DPoint < nbTotalInnerCorners; index3DPoint++)
		{
			cvmSet(objectPoints,index3DPoint,0,pattern.GetPatternPoints()[index3DPoint%pattern.GetNumberOfInnerCorners()].x);
			cvmSet(objectPoints,index3DPoint,1,pattern.GetPatternPoints()[index3DPoint%pattern.GetNumberOfInnerCorners()].y);
			cvmSet(objectPoints,index3DPoint,2,pattern.GetPatternPoints()[index3DPoint%pattern.GetNumberOfInnerCorners()].z);
		}
				
		// Calcul des paramètres intrinsèques et extrinsèques de la caméra 
		CvMat * intrinsicMatrix;
		CvMat * distortionCofficients;		
		CvMat * rotationVectors;
		CvMat * translationVectors;		
		intrinsicMatrix = cvCreateMat(3,3,CV_32FC1);
		distortionCofficients = cvCreateMat(1,4,CV_32FC1);
		rotationVectors = cvCreateMat(data.GetNumberOfImages(),3,CV_32FC1);
		translationVectors = cvCreateMat(data.GetNumberOfImages(),3,CV_32FC1);		
		try
		{					
			cvCalibrateCamera2( 	objectPoints, imagePoints, pointCounts,
						cvSize(data.GetWidthOfImages(),data.GetHeightOfImages()),
						intrinsicMatrix, distortionCofficients, rotationVectors,translationVectors, 0);

			// Extraction des parametres extrinseques
			CvMat* compactRotationMatrix =  cvCreateMat(1, 3, CV_32FC1);
			CvMat* expendedRotationMatrix = cvCreateMat(3, 3, CV_32FC1); 
			float rotationMatrix[16];
			float translationMatrix[16];
			for( int imageIndex=0;  imageIndex<data.GetNumberOfImages(); ++imageIndex )
			{
				cvmSet( compactRotationMatrix, 0, 0, cvmGet( rotationVectors, imageIndex, 0 ) );
				cvmSet( compactRotationMatrix, 0, 1, cvmGet( rotationVectors, imageIndex, 1 ) );
				cvmSet( compactRotationMatrix, 0, 2, cvmGet( rotationVectors, imageIndex, 2 ) );
				cvRodrigues2( compactRotationMatrix, expendedRotationMatrix, 0 );
				
				// Construction matrice translation
				Tools::mat44_identity(translationMatrix);
				Tools::mat44_translate(	translationMatrix, 
							cvmGet( translationVectors, imageIndex, 0 ),
							cvmGet( translationVectors, imageIndex, 1 ),
							cvmGet( translationVectors, imageIndex, 2 ) );

				// Conversion de la matrice OpenCV -> column major
				rotationMatrix[0] = cvmGet( expendedRotationMatrix, 0, 0 );
				rotationMatrix[1] = cvmGet( expendedRotationMatrix, 1, 0 );
				rotationMatrix[2] = cvmGet( expendedRotationMatrix, 2, 0 );
				rotationMatrix[3] = 0;
				rotationMatrix[4] = cvmGet( expendedRotationMatrix, 0, 1 );
				rotationMatrix[5] = cvmGet( expendedRotationMatrix, 1, 1 );
				rotationMatrix[6] = cvmGet( expendedRotationMatrix, 2, 1 );
				rotationMatrix[7] = 0;
				rotationMatrix[8] = cvmGet( expendedRotationMatrix, 0, 2 );
				rotationMatrix[9] = cvmGet( expendedRotationMatrix, 1, 2 );
				rotationMatrix[10] = cvmGet( expendedRotationMatrix, 2, 2 );
				rotationMatrix[11] = 0;
				rotationMatrix[12] = 0;
				rotationMatrix[13] = 0;
				rotationMatrix[14] = 0;
				rotationMatrix[15] = 1;

				positionSet.AddPosition( translationMatrix, rotationMatrix, imageIndex );
			}
			cvReleaseMat(&expendedRotationMatrix);
			cvReleaseMat(&compactRotationMatrix);
		}
		catch( std::exception& e )
		{			
			throw Tools::CalibrationFailed(e.what());
		}
		
		// Affichage des paramètres de calibrage
		// Tools::ILogger::GetLogger("Calibration")->Info("Intrinsec Parameters : [%6.2f,%6.2f,%6.2f;%6.2f,%6.2f,%6.2f;%6.2f,%6.2f,%6.2f]\n",
		//  												cvmGet(intrinsicMatrix,0,0), cvmGet(intrinsicMatrix,0,1), cvmGet(intrinsicMatrix,0,2),			
		//		   										cvmGet(intrinsicMatrix,1,0), cvmGet(intrinsicMatrix,1,1), cvmGet(intrinsicMatrix,1,2),
		//		   										cvmGet(intrinsicMatrix,2,0), cvmGet(intrinsicMatrix,2,1), cvmGet(intrinsicMatrix,2,2) );		 
		// Tools::ILogger::GetLogger("Calibration")->Info("Distotion Coefficients : [%6.2f,%6.2f,%6.2f,%6.2f]\n", 
		//												cvmGet(distortionCofficients,0,0),cvmGet(distortionCofficients,0,1),
		//												cvmGet(distortionCofficients,0,2),cvmGet(distortionCofficients,0,3) );  
		// Tools::ILogger::GetLogger("Calibration")->Info("Rotation Vector : [%6.2f,%6.2f,%6.2f]\n",
		//												cvmGet(rotationVectors,0,0),cvmGet(rotationVectors,0,1),cvmGet(rotationVectors,0,2) );														
		// Tools::ILogger::GetLogger("Calibration")->Info("Translation Vector : [%6.2f,%6.2f,%6.2f]\n",
		//												cvmGet(translationVectors,0,0),cvmGet(translationVectors,0,1),cvmGet(translationVectors,0,2) );																
		
		// Affectation des paramètres de calibrage à la caméra
		camera.SetCameraCalibrationParameter( intrinsicMatrix, distortionCofficients );
		
		// Libération de l'ensemble des ressources allouées
		cvReleaseMat(&translationVectors);
		cvReleaseMat(&rotationVectors);
		cvReleaseMat(&distortionCofficients);		
		cvReleaseMat(&intrinsicMatrix);

		//cvReleaseMat(&objectPoints);
		cvReleaseMat(&pointCounts);
		cvReleaseMat(&imagePoints);		
	}	
}
